#include "IRcom.h"

void ir_com_init()
{
    //// PD0 (Carrier wave) and PC7 (TX) as output
    //PORTC.DIRSET   |= PIN4_bm;
	//USARTPORT.DIRSET |= PIN7_bm;
	//USARTPORT.PIN7CTRL |= PORT_INVEN_bm;
    //// PC6 (RX) as input
    //USARTPORT.DIRCLR   = PIN6_bm; 
	// Activate C0 (TX = 3, RX = 2), C1 (TX = 7, RX = 6), D0 (TX = 3, RX = 2), D1 (TX = 7, RX = 6)
	PORTC.DIRSET |= PIN3_bm; PORTC.PIN3CTRL |= PORT_INVEN_bm; PORTC.DIRCLR |= PIN2_bm;
	PORTC.DIRSET |= PIN7_bm; PORTC.PIN7CTRL |= PORT_INVEN_bm; PORTC.DIRCLR |= PIN6_bm;
	PORTD.DIRSET |= PIN3_bm; PORTD.PIN3CTRL |= PORT_INVEN_bm; PORTD.DIRCLR |= PIN2_bm;
	PORTD.DIRSET |= PIN7_bm; PORTD.PIN7CTRL |= PORT_INVEN_bm; PORTD.DIRCLR |= PIN6_bm;
	
	// For Molecule V3, these correspond to the enumerations NORTH, SOUTH, EAST, WEST.
	USARTptrs[NORTH] = &USARTC1; USARTptrs[SOUTH] = &USARTD1; USARTptrs[EAST] = &USARTC0; USARTptrs[WEST] = &USARTD0;

	// Carrier wave generation
    TCC1.CTRLB = TC_WGMODE_SS_gc; // Single Slope PWM as Waveform Generation mode. 
    TC1_ConfigClockSource(&TCC1, TC_CLKSEL_DIV1_gc);
    TC_SetPeriod(&TCC1, 421);     // 32MHz / 2 / 421 = 38KHz
    TC_SetCompareA(&TCC1, 210);
    TCC1.CTRLB |= TC1_CCAEN_bm;   // Enable the CC channel

	for (int i = 0; i < 4; i++) {
		// Use USARTC1 and initialize buffers
		USART_InterruptDriver_Initialize(&USART_data[i], USARTptrs[i], USART_DREINTLVL_LO_gc);
		// USARTC1, 8 Data bits, No Parity, 1 Stop bit
		USART_Format_Set(USART_data[i].usart, USART_CHSIZE_8BIT_gc,
						 USART_PMODE_DISABLED_gc, false);
		// Enable RXC and TXC interrupts
		USART_RxdInterruptLevel_Set(USART_data[i].usart, USART_RXCINTLVL_LO_gc);
		//USART_TxdInterruptLevel_Set(USART_data[i].usart, USART_TXCINTLVL_LO_gc);
		//USART.CTRLA |= USART_TXCINTLVL_LO_gc;
		USART_Baudrate_Set(USARTptrs[i], 3329, -3);  // 2400 Baud

		/* Enable both RX and TX. */
		USART_Rx_Enable(USART_data[i].usart);
		USART_Tx_Enable(USART_data[i].usart);
	}	

	// Enable PMIC interrupt level low
	PMIC.CTRL |= PMIC_LOLVLEX_bm;

	// Enable global interrupts
	sei();
}

//  Receive complete interrupt service routine.
ISR( USARTC0_RXC_vect )
{
    USART_RXComplete( &USART_data[EAST] );
	uint8_t inByte = USART_RXBuffer_GetByte(&USART_data[EAST]);
	ReceivedByte(inByte,EAST);
}
//  Data register empty  interrupt service routine.
ISR( USARTC0_DRE_vect )
{
    USART_DataRegEmpty( &USART_data[EAST] );
	TransmittedByte(EAST);
	/* Check if all data is transmitted. */
	uint8_t tempTX_Tail = USART_data[EAST].buffer.TX_Tail;
	if (USART_data[EAST].buffer.TX_Head == tempTX_Tail) PrepareOutgoingPacket(EAST);
}

//  Receive complete interrupt service routine.
ISR( USARTC1_RXC_vect )
{
    USART_RXComplete( &USART_data[NORTH] );
	uint8_t inByte = USART_RXBuffer_GetByte(&USART_data[NORTH]);
	ReceivedByte(inByte,NORTH);
}
//  Data register empty  interrupt service routine.
ISR( USARTC1_DRE_vect )
{
    USART_DataRegEmpty( &USART_data[NORTH] );
	TransmittedByte(NORTH);
	/* Check if all data is transmitted. */
	uint8_t tempTX_Tail = USART_data[NORTH].buffer.TX_Tail;
	if (USART_data[NORTH].buffer.TX_Head == tempTX_Tail) PrepareOutgoingPacket(NORTH);
}

//  Receive complete interrupt service routine.
ISR( USARTD0_RXC_vect )
{
    USART_RXComplete( &USART_data[WEST] );
	uint8_t inByte = USART_RXBuffer_GetByte(&USART_data[WEST]);
	ReceivedByte(inByte,WEST);
}
//  Data register empty  interrupt service routine.
ISR( USARTD0_DRE_vect )
{
    USART_DataRegEmpty( &USART_data[WEST] );
	TransmittedByte(WEST);
	/* Check if all data is transmitted. */
	uint8_t tempTX_Tail = USART_data[WEST].buffer.TX_Tail;
	if (USART_data[WEST].buffer.TX_Head == tempTX_Tail) PrepareOutgoingPacket(WEST);
}

//  Receive complete interrupt service routine.
ISR( USARTD1_RXC_vect )
{
    USART_RXComplete( &USART_data[SOUTH] );
	uint8_t inByte = USART_RXBuffer_GetByte(&USART_data[SOUTH]);
	ReceivedByte(inByte,SOUTH);
}
//  Data register empty  interrupt service routine.
ISR( USARTD1_DRE_vect )
{
    USART_DataRegEmpty( &USART_data[SOUTH] );
	TransmittedByte(SOUTH);
	/* Check if all data is transmitted. */
	uint8_t tempTX_Tail = USART_data[SOUTH].buffer.TX_Tail;
	if (USART_data[SOUTH].buffer.TX_Head == tempTX_Tail) PrepareOutgoingPacket(SOUTH);
}